Import cal3d

Class CalMatrix

	Field dxdx:Float,dydx:Float,dzdx:Float
	Field dxdy:Float,dydy:Float,dzdy:Float
	Field dxdz:Float,dydz:Float,dzdz:Float

	

	Method New()
	
		dxdx = 1
		dydx = 0
		dzdx = 0
		dxdy = 0
		dydy = 1
		dzdy = 0
		dxdz = 0
		dydz = 0
		dzdz = 1
		
		'assign.apply(this, arguments)
			
	End
	
	
	Method New(m:CalMatrix)
			dxdx = 1
			dydx = 0
			dzdx = 0
			dxdy = 0
			dydy = 1
			dzdy = 0
			dxdz = 0
			dydz = 0
			dzdz = 1	
			dxdx = m.dxdx; dydx = m.dydx; dzdx = m.dzdx;
			dxdy = m.dxdy; dydy = m.dydy; dzdy = m.dzdy;
			dxdz = m.dxdz; dydz = m.dydz; dzdz = m.dzdz;	
	End
	
	Method assign(q:CalQuaternion)
			Local xx2:Float = q.x * q.x * 2
			Local yy2:Float = q.y * q.y * 2
			Local zz2:Float = q.z * q.z * 2
			Local xy2:Float = q.x * q.y * 2
			Local zw2:Float = q.z * q.w * 2
			Local xz2:Float = q.x * q.z * 2
			Local yw2:Float = q.y * q.w * 2
			Local yz2:Float = q.y * q.z * 2
			Local xw2:Float = q.x * q.w * 2
			dxdx = 1-yy2-zz2   
			dxdy =   xy2+zw2  
			dxdz =   xz2-yw2
			dydx =   xy2-zw2   
			dydy = 1-xx2-zz2  
			dydz =   yz2+xw2
			dzdx =   xz2+yw2   
			dzdy =   yz2-xw2  
			dzdz = 1-xx2-yy2	
	End
	
	Method multMatrixLocal:CalMatrix(m:CalMatrix)
		Local ndxdx:Float = m.dxdx*dxdx+m.dxdy*dydx+m.dxdz*dzdx
		local ndydx:Float = m.dydx*dxdx+m.dydy*dydx+m.dydz*dzdx
		local ndzdx:Float = m.dzdx*dxdx+m.dzdy*dydx+m.dzdz*dzdx
	
		local ndxdy:Float = m.dxdx*dxdy+m.dxdy*dydy+m.dxdz*dzdy
		local ndydy:Float = m.dydx*dxdy+m.dydy*dydy+m.dydz*dzdy
		local ndzdy:Float = m.dzdx*dxdy+m.dzdy*dydy+m.dzdz*dzdy
	
		local ndxdz:Float = m.dxdx*dxdz+m.dxdy*dydz+m.dxdz*dzdz
		local ndydz:Float = m.dydx*dxdz+m.dydy*dydz+m.dydz*dzdz
		Local ndzdz:Float = m.dzdx*dxdz+m.dzdy*dydz+m.dzdz*dzdz	
		
		dxdx = ndxdx
		dydx = ndydx
		dzdx = ndzdx
		dxdy = ndxdy
		dydy = ndydy
		dzdy = ndzdy
		dxdz = ndxdz
		dydz = ndydz
		dzdz = ndzdz	
		
		Return Self	
		
	End
	
	
	Method multScalarLocal:CalMatrix(factor:Float)

		dxdx *= factor
		dydx *= factor
		dzdx *= factor
		dxdy *= factor
		dydy *= factor
		dzdy *= factor
		dxdz *= factor
		dydz *= factor
		dzdz *= factor
	
		return self	
	
	
	End
	
	Method blend:Void(factor:Float, m:CalMatrix)
	
		dxdx += m.dxdx*factor
		dydx += m.dydx*factor
		dzdx += m.dzdx*factor
		dxdy += m.dxdy*factor
		dydy += m.dydy*factor
		dzdy += m.dzdy*factor
		dxdz += m.dxdz*factor
		dydz += m.dydz*factor
		dzdz += m.dzdz*factor	
	
	End
	
	Method det:Float()
		return dxdx * (dydy*dzdz-dydz*dzdy) - dxdy * (dydx*dzdz-dzdx*dydz) + dxdz * (dydx*dzdy-dzdx*dydy)	
	End
	
End Class



Class CalQuaternion
	Field x:Float,y:Float,z:Float,w:Float
	
	Method New()
		x = 0
		y = 0
		z = 0
		w = 1
	End

	Method New(other:CalQuaternion)
		x = other.x
		y = other.y
		z = other.z
		w = other.w
	End

	Method New(xx:Float,yy:Float,zz:Float,ww:Float)
		x = xx
		y = yy
		z = zz
		w = ww
	End


	Method assign:Void(other:CalQuaternion)
		x = other.x
		y = other.y
		z = other.z
		w = other.w
	End
	
	
	Method multQuaternionLocal:CalQuaternion(q:CalQuaternion)
		local qx:Float = x
		local qy:Float = y
		local qz:Float = z
		local qw:Float = w
		
		x = qw * q.x + qx * q.w + qy * q.z - qz * q.y
		y = qw * q.y - qx * q.z + qy * q.w + qz * q.x
		z = qw * q.z + qx * q.y - qy * q.x + qz * q.w
		w = qw * q.w - qx * q.x - qy * q.y - qz * q.z
	
		return self		
	
	End

	Method multVectorLocal:CalQuaternion(v:CalVector)
		Local qx:Float = x
		local qy:Float = y
		local qz:Float = z
		local qw:Float = w
		
		x = qw * v.x + qy * v.z - qz * v.y
		y = qw * v.y - qx * v.z + qz * v.x
		z = qw * v.z + qx * v.y - qy * v.x
		w = -qx * v.x - qy * v.y - qz * v.z
	
		return self		
	End

	Method equalTo:Bool(q:CalQuaternion)
	
		If x = q.x And y = q.y And z = q.z And w = q.w Then
			Return True
		Else
			Return false
		Endif
	End
	
	Method blend:Void(d:Float, q:CalQuaternion)
		local norm:Float = x * q.x + y * q.y + z * q.z + w * q.w
		
		Local bFlip:Bool = False	
		if(norm < 0)
		
			norm = -norm
			bFlip = true
		endif
		
		Local inv_d:Float
		if(1 - norm < 1e-6)
		
			inv_d = 1 - d
		
		else
		
			Local theta:Float = ACos(norm)
			Local s:Float = 1 / Sin(theta)
			
			inv_d = Sin((1 - d) * theta) * s
			d = Sin(d * theta) * s
		endif
		
		if(bFlip)
		
			d = -d
		endif
		
		x = inv_d * x + d * q.x
		y = inv_d * y + d * q.y
		z = inv_d * z + d * q.z
		w = inv_d * w + d * q.w	
		
	
	End

	Method clear:Void()
		x = 0
		y = 0
		z = 0
		w = 1		
	End

	Method conjugate:Void()
		x = -x
		y = -y
		z = -z	
	End

	Method invert:Void()
		conjugate()
	
		Local norm:Float = (x*x) + (y*y) + (z*z) + (w*w)	
		
		if (norm = 0) 
			return
		Endif
		
		Local inv_norm:Float = 1 / norm
		x *= inv_norm
		y *= inv_norm
		z *= inv_norm
		w *= inv_norm	
	
	End



End Class

Function quaternionMult:CalQuaternion(q0:CalQuaternion, q1:CalQuaternion)

	Return New CalQuaternion(q1.w * q0.x + q1.x * q0.w + q1.y * q0.z - q1.z * q0.y,q1.w * q0.y - q1.x * q0.z + q1.y * q0.w + q1.z * q0.x,q1.w * q0.z + q1.x * q0.y - q1.y * q0.x + q1.z * q0.w,q1.w * q0.w - q1.x * q0.x - q1.y * q0.y - q1.z * q0.z)

End

Function shortestArc:CalQuaternion(from:Float, tto:Float)
	Local cp:CalVector = vectorCross(from, tto) 
	Local dp:Float = vectorDot(from, tto)
	
	' we will use this equation twice
	dp = Sqrt( 2*(dp+1) ) 
	
	' get the x, y, z components
	cp.divScalarLocal(dp)
	
	' return with the w component (Note that w is inverted because Cal3D has left-handed rotations)
	return new CalQuaternion( cp.x, cp.y, cp.z, -dp/2 )

End
	

Class CalVector

	Field x:Float,y:Float,z:Float
	
	Method New()
	
		x = 0
		y = 0
		z = 0
	
	End
	
	Method New(other:CalVector)
		x = other.x
		y = other.y
		z = other.z		
	End
	
	Method New(xx:Float,yy:Float,zz:Float)
	
		x = xx
		y = yy
		z = zz
	
	End

	
	Method assign:Void(v:CalVertex)
	
		x = v.position.x
		y = v.position.y
		z = v.position.z
		
	
	End
	
	Method assign:Void(xx:Float,yy:Float,zz:Float)
	
		x = xx
		y = yy
		z = zz
	
	End
	
	Method assign:Void(v:CalVector)
	
		x = v.x
		y = v.y
		z = v.z
	
	End	
	
	
	Method addLocal:CalVector(v:CalVector)
	
		x += v.x
		y += v.y
		z += v.z
	
		return self		
	
	End

	Method subLocal:CalVector(v:CalVector)
	
		x -= v.x
		y -= v.y
		z -= v.z
	
		return self		
	
	End
	
	Method multScalarLocal:CalVector(factor:Float)
	
		x *= factor
		y *= factor
		z *= factor
	
		return self		
	
	End


	Method multQuaternionLocal:CalVector(q:CalQuaternion)
		
		local temp:CalQuaternion = new CalQuaternion(-q.x, -q.y, -q.z, q.w)
		temp.multVectorLocal(self).multQuaternionLocal(q)
	
		x = temp.x
		y = temp.y
		z = temp.z
	
		return self	
	End

	Method multMatrixLocal:CalVector(m:CalMatrix)
	
		Local ox:Float = x
		local oy:Float = y
		local oz:Float = z
		x = m.dxdx*ox + m.dxdy*oy + m.dxdz*oz
		y = m.dydx*ox + m.dydy*oy + m.dydz*oz
		z = m.dzdx*ox + m.dzdy*oy + m.dzdz*oz
	
		return self	
	
	End

	Method blend:Void(d:Float, v:CalVector)
		x += d * (v.x - x)
		y += d * (v.y - y)
		z += d * (v.z - z)
	End


	Method divScalarLocal:CalVector(factor:Float)
	
		x /= factor
		y /= factor
		z /= factor
	
		return self	
	
	End

	Method equalTo:Bool(v:CalVector)
	
		If x = v.x And y = v.y And z = v.z Then
			Return True
		Else
			Return False
		Endif
	
	End

	Method clear:Void()
		x = 0
		y = 0
		z = 0
	End

	Method length:Float()
	
		return Sqrt(x * x + y * y + z * z)
	
	End
	
	Method normalize:Float()

		Local len:float = Sqrt(x * x + y * y + z * z)
		x /= len
		y /= len
		z /= len
	
		return len
	
	End
	
End Class


Function vectorAdd:CalVector(v0:CalVector, v1:CalVector)

	return new CalVector(v0.x + v1.x, v0.y + v1.y, v0.z + v1.z)
End

Function vectorSub:CalVector(v0:CalVector, v1:CalVector)

	Return New CalVector(v0.x - v1.x, v0.y - v1.y, v0.z - v1.z)

End

Function vectorSub:CalVector(v0:CalVertex, v1:CalVertex)

	Return New CalVector(v0.position.x - v1.position.x, v0.position.y - v1.position.y, v0.position.z - v1.position.z)

End

Function vectorScalarMult:CalVector(v:CalVector, d:Float)

	Return New CalVector(v.x * d, v.y * d, v.z * d)	
	
End

Function vectorScalarDiv:CalVector(v:CalVector, d:Float)

	Return New CalVector(v.x / d, v.y / d, v.z / d)
	
End

Function vectorDot:Float(v0:CalVector, v1:CalVector)

	return v0.x * v1.x + v0.y * v1.y + v0.z * v1.z

End

Function vectorCross:CalVector(v0:CalVector, v1:CalVector)

	return new CalVector(v0.y * v1.z - v0.z * v1.y, v0.z * v1.x - v0.x * v1.z, v0.x * v1.y - v0.y * v1.x)

End